function zmp = computeZMP(theta,theta_dot,torque)
% Compute Zero Moment Point by Forward Kinematics
% Paper: ZERO-MOMENT POINT METHOD FOR STABLE BIPED WALKING. Author: M.H.P. Dekker. Eindhoven, Izzuly 2009 DCT no.: 2009.072. Page 12. 
% Last modification: 30/03/2014 (by Feliphe G. Galiza)

qd = [theta_dot(1); theta_dot(2); theta_dot(3); theta_dot(4); theta_dot(5)];

theta1 = theta(1);
theta2 = theta(2);
theta3 = theta(3);
theta4 = theta(4);
theta5 = theta(5);

theta1_dot = theta_dot(1);
theta2_dot = theta_dot(2);
theta3_dot = theta_dot(3);
theta4_dot = theta_dot(4);
theta5_dot = theta_dot(5);

u1 = torque(1);
u2 = torque(2);
u3 = torque(3);
u4 = torque(4);
u5 = torque(5);

g = 9.81; % gravity [m/s^2]

% parameters according to human body
height = 1.5; % [m]
L1 = 0.285*height; % [m]
L2 = (0.53 - 0.285)*height; % [m]
L3 = height - (L1 + L2); % [m]
L4 = L2; % [m]
L5 =L1; % [m]

r1 = L1/2; % [m]
r2 = L2/2; % [m]
r3 = L3/2; % [m]    
r4 = L4/2; % [m]
r5 = L5/2; % [m]

mass = 50; % [kg]
m1 = 0.061 * mass; % [kg]
m2 = 0.1 * mass; % [kg]
m3 = 0.678 * mass; % [kg]
m4 = m2; % [kg]
m5 = m1; % [kg]
m = [m1 m2 m3 m4 m5];

Izz_1 = m1 * 0.735^2; % [kg*m^2]
Izz_2 = m2 * 0.540^2; % [kg*m^2]
Izz_3 = m3 * 0.0798^2; % [kg*m^2]
Izz_4 = Izz_2; % [kg*m^2]
Izz_5 = Izz_1; % [kg*m^2]

Izz = [Izz_1 Izz_2 Izz_3 Izz_4 Izz_5];

%[M,RHS] = eqOfmotion(theta_pos,theta_dot,torque);

% Using Newton-Euler
M(1,:) = [ Izz_1 + Izz_2 + Izz_3 + Izz_4 + Izz_5 + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1) + r4*sin(theta1 + theta2 + theta3 - theta4))^2 + m4*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4))^2 + m3*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) + r3*cos(theta1 + theta2 + theta3))^2 + m4*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1) - r4*sin(theta1 + theta2 + theta3 - theta4))^2 + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) - cos(theta1)*(L1 - r1) + r4*cos(theta1 + theta2 + theta3 - theta4))^2 + m2*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1))^2 + m2*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1))^2 + m3*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1))^2 + m1*cos(theta1)^2*(L1 - r1)^2 + m1*sin(theta1)^2*(L1 - r1)^2, Izz_2 + Izz_3 + Izz_4 + Izz_5 + L2^2*m2 + L2^2*m3 + L2^2*m4 + L2^2*m5 + m2*r2^2 + m3*r2^2 + m3*r3^2 + m4*r2^2 + m5*r2^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - 2*L2*m2*r2 - 2*L2*m3*r2 - 2*L2*m4*r2 - 2*L2*m5*r2 - L1*m4*r4*cos(theta2 + theta3 - theta4) - L1*m5*r4*cos(theta2 + theta3 - theta4) - 2*L2*m5*r5*cos(theta3 - theta4 + theta5) + m4*r1*r4*cos(theta2 + theta3 - theta4) + m5*r1*r4*cos(theta2 + theta3 - theta4) + 2*m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m3*r3*cos(theta2 + theta3) + L1*L2*m2*cos(theta2) + L1*L2*m3*cos(theta2) + L1*L2*m4*cos(theta2) + L1*L2*m5*cos(theta2) - m3*r1*r3*cos(theta2 + theta3) - L1*m2*r2*cos(theta2) - L2*m2*r1*cos(theta2) - L1*m3*r2*cos(theta2) - L2*m3*r1*cos(theta2) - L1*m4*r2*cos(theta2) - L2*m4*r1*cos(theta2) - L1*m5*r2*cos(theta2) - L2*m5*r1*cos(theta2) + 2*L2*m3*r3*cos(theta3) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) + m2*r1*r2*cos(theta2) + m3*r1*r2*cos(theta2) + m4*r1*r2*cos(theta2) + m5*r1*r2*cos(theta2) - 2*m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - 2*L2*m4*r4*cos(theta3 - theta4) - 2*L2*m5*r4*cos(theta3 - theta4) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) + 2*m4*r2*r4*cos(theta3 - theta4) + 2*m5*r2*r4*cos(theta3 - theta4), Izz_3 + Izz_4 + Izz_5 + m3*r3^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - L1*m4*r4*cos(theta2 + theta3 - theta4) - L1*m5*r4*cos(theta2 + theta3 - theta4) - L2*m5*r5*cos(theta3 - theta4 + theta5) + m4*r1*r4*cos(theta2 + theta3 - theta4) + m5*r1*r4*cos(theta2 + theta3 - theta4) + m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m3*r3*cos(theta2 + theta3) - m3*r1*r3*cos(theta2 + theta3) + L2*m3*r3*cos(theta3) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) - m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - L2*m4*r4*cos(theta3 - theta4) - L2*m5*r4*cos(theta3 - theta4) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) + m4*r2*r4*cos(theta3 - theta4) + m5*r2*r4*cos(theta3 - theta4), L1*m4*r4*cos(theta2 + theta3 - theta4) - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - Izz_4 + L1*m5*r4*cos(theta2 + theta3 - theta4) + L2*m5*r5*cos(theta3 - theta4 + theta5) - m4*r1*r4*cos(theta2 + theta3 - theta4) - m5*r1*r4*cos(theta2 + theta3 - theta4) - m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) - 2*m5*r4*r5*cos(theta5) + L2*m4*r4*cos(theta3 - theta4) + L2*m5*r4*cos(theta3 - theta4) - m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) - m4*r2*r4*cos(theta3 - theta4) - m5*r2*r4*cos(theta3 - theta4), Izz_5 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) + m5*r4*r5*cos(theta5) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5)];
M(2,:) = [ Izz_2 + Izz_3 + Izz_4 + Izz_5 + L2^2*m2 + L2^2*m3 + L2^2*m4 + L2^2*m5 + m2*r2^2 + m3*r2^2 + m3*r3^2 + m4*r2^2 + m5*r2^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - 2*L2*m2*r2 - 2*L2*m3*r2 - 2*L2*m4*r2 - 2*L2*m5*r2 - L1*m4*r4*cos(theta2 + theta3 - theta4) - L1*m5*r4*cos(theta2 + theta3 - theta4) - 2*L2*m5*r5*cos(theta3 - theta4 + theta5) + m4*r1*r4*cos(theta2 + theta3 - theta4) + m5*r1*r4*cos(theta2 + theta3 - theta4) + 2*m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m3*r3*cos(theta2 + theta3) + L1*L2*m2*cos(theta2) + L1*L2*m3*cos(theta2) + L1*L2*m4*cos(theta2) + L1*L2*m5*cos(theta2) - m3*r1*r3*cos(theta2 + theta3) - L1*m2*r2*cos(theta2) - L2*m2*r1*cos(theta2) - L1*m3*r2*cos(theta2) - L2*m3*r1*cos(theta2) - L1*m4*r2*cos(theta2) - L2*m4*r1*cos(theta2) - L1*m5*r2*cos(theta2) - L2*m5*r1*cos(theta2) + 2*L2*m3*r3*cos(theta3) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) + m2*r1*r2*cos(theta2) + m3*r1*r2*cos(theta2) + m4*r1*r2*cos(theta2) + m5*r1*r2*cos(theta2) - 2*m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - 2*L2*m4*r4*cos(theta3 - theta4) - 2*L2*m5*r4*cos(theta3 - theta4) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) + 2*m4*r2*r4*cos(theta3 - theta4) + 2*m5*r2*r4*cos(theta3 - theta4), Izz_2 + Izz_3 + Izz_4 + Izz_5 + L2^2*m2 + L2^2*m3 + L2^2*m4 + L2^2*m5 + m2*r2^2 + m3*r2^2 + m3*r3^2 + m4*r2^2 + m5*r2^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - 2*L2*m2*r2 - 2*L2*m3*r2 - 2*L2*m4*r2 - 2*L2*m5*r2 - 2*L2*m5*r5*cos(theta3 - theta4 + theta5) + 2*m5*r2*r5*cos(theta3 - theta4 + theta5) + 2*L2*m3*r3*cos(theta3) - 2*m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - 2*L2*m4*r4*cos(theta3 - theta4) - 2*L2*m5*r4*cos(theta3 - theta4) + 2*m4*r2*r4*cos(theta3 - theta4) + 2*m5*r2*r4*cos(theta3 - theta4), Izz_3 + Izz_4 + Izz_5 + m3*r3^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) + L2*m3*r3*cos(theta3) - m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - L2*m4*r4*cos(theta3 - theta4) - L2*m5*r4*cos(theta3 - theta4) + m4*r2*r4*cos(theta3 - theta4) + m5*r2*r4*cos(theta3 - theta4), L2*m5*r5*cos(theta3 - theta4 + theta5) - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - Izz_4 - m5*r2*r5*cos(theta3 - theta4 + theta5) - 2*m5*r4*r5*cos(theta5) + L2*m4*r4*cos(theta3 - theta4) + L2*m5*r4*cos(theta3 - theta4) - m4*r2*r4*cos(theta3 - theta4) - m5*r2*r4*cos(theta3 - theta4), Izz_5 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) + m5*r4*r5*cos(theta5)];
M(3,:) = [ Izz_3 + Izz_4 + Izz_5 + m3*r3^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - L1*m4*r4*cos(theta2 + theta3 - theta4) - L1*m5*r4*cos(theta2 + theta3 - theta4) - L2*m5*r5*cos(theta3 - theta4 + theta5) + m4*r1*r4*cos(theta2 + theta3 - theta4) + m5*r1*r4*cos(theta2 + theta3 - theta4) + m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m3*r3*cos(theta2 + theta3) - m3*r1*r3*cos(theta2 + theta3) + L2*m3*r3*cos(theta3) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) - m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - L2*m4*r4*cos(theta3 - theta4) - L2*m5*r4*cos(theta3 - theta4) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) + m4*r2*r4*cos(theta3 - theta4) + m5*r2*r4*cos(theta3 - theta4), Izz_3 + Izz_4 + Izz_5 + m3*r3^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) + L2*m3*r3*cos(theta3) - m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - L2*m4*r4*cos(theta3 - theta4) - L2*m5*r4*cos(theta3 - theta4) + m4*r2*r4*cos(theta3 - theta4) + m5*r2*r4*cos(theta3 - theta4), Izz_3 + Izz_4 + Izz_5 + m3*r3^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 + 2*m5*r4*r5*cos(theta5), - Izz_4 - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - 2*m5*r4*r5*cos(theta5), m5*r5^2 + m5*r4*cos(theta5)*r5 + Izz_5];
M(4,:) = [ L1*m4*r4*cos(theta2 + theta3 - theta4) - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - Izz_4 + L1*m5*r4*cos(theta2 + theta3 - theta4) + L2*m5*r5*cos(theta3 - theta4 + theta5) - m4*r1*r4*cos(theta2 + theta3 - theta4) - m5*r1*r4*cos(theta2 + theta3 - theta4) - m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) - 2*m5*r4*r5*cos(theta5) + L2*m4*r4*cos(theta3 - theta4) + L2*m5*r4*cos(theta3 - theta4) - m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) - m4*r2*r4*cos(theta3 - theta4) - m5*r2*r4*cos(theta3 - theta4), L2*m5*r5*cos(theta3 - theta4 + theta5) - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - Izz_4 - m5*r2*r5*cos(theta3 - theta4 + theta5) - 2*m5*r4*r5*cos(theta5) + L2*m4*r4*cos(theta3 - theta4) + L2*m5*r4*cos(theta3 - theta4) - m4*r2*r4*cos(theta3 - theta4) - m5*r2*r4*cos(theta3 - theta4), - Izz_4 - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - 2*m5*r4*r5*cos(theta5), Izz_4 + Izz_5 + m4*r4^2 + m5*r4^2 + m5*r5^2 + 2*m5*r4*r5*cos(theta5), - m5*r5^2 - m5*r4*cos(theta5)*r5 - Izz_5];
M(5,:) = [ Izz_5 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) + m5*r4*r5*cos(theta5) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5), Izz_5 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) + m5*r4*r5*cos(theta5), m5*r5^2 + m5*r4*cos(theta5)*r5 + Izz_5, - m5*r5^2 - m5*r4*cos(theta5)*r5 - Izz_5, m5*r5^2 + Izz_5];

RHS(1,1) = u1 - g*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1) + r4*sin(theta1 + theta2 + theta3 - theta4)) + g*m4*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + g*m2*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1)) + g*m3*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1)) + g*m1*sin(theta1)*(L1 - r1) + m4*r1*r4*theta2_dot^2*sin(theta2 + theta3 - theta4) + m4*r1*r4*theta3_dot^2*sin(theta2 + theta3 - theta4) + m5*r1*r4*theta2_dot^2*sin(theta2 + theta3 - theta4) + m4*r1*r4*theta4_dot^2*sin(theta2 + theta3 - theta4) + m5*r1*r4*theta3_dot^2*sin(theta2 + theta3 - theta4) + m5*r1*r4*theta4_dot^2*sin(theta2 + theta3 - theta4) + m5*r2*r5*theta3_dot^2*sin(theta3 - theta4 + theta5) + m5*r2*r5*theta4_dot^2*sin(theta3 - theta4 + theta5) + m5*r2*r5*theta5_dot^2*sin(theta3 - theta4 + theta5) + L1*m3*r3*theta2_dot^2*sin(theta2 + theta3) + L1*m3*r3*theta3_dot^2*sin(theta2 + theta3) + L1*L2*m2*theta2_dot^2*sin(theta2) + L1*L2*m3*theta2_dot^2*sin(theta2) + L1*L2*m4*theta2_dot^2*sin(theta2) + L1*L2*m5*theta2_dot^2*sin(theta2) - m3*r1*r3*theta2_dot^2*sin(theta2 + theta3) - m3*r1*r3*theta3_dot^2*sin(theta2 + theta3) - L1*m2*r2*theta2_dot^2*sin(theta2) - L2*m2*r1*theta2_dot^2*sin(theta2) - L1*m3*r2*theta2_dot^2*sin(theta2) - L2*m3*r1*theta2_dot^2*sin(theta2) - L1*m4*r2*theta2_dot^2*sin(theta2) - L2*m4*r1*theta2_dot^2*sin(theta2) - L1*m5*r2*theta2_dot^2*sin(theta2) - L2*m5*r1*theta2_dot^2*sin(theta2) + L2*m3*r3*theta3_dot^2*sin(theta3) - L1*m5*r5*theta2_dot^2*sin(theta2 + theta3 - theta4 + theta5) - L1*m5*r5*theta3_dot^2*sin(theta2 + theta3 - theta4 + theta5) - L1*m5*r5*theta4_dot^2*sin(theta2 + theta3 - theta4 + theta5) - L1*m5*r5*theta5_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m2*r1*r2*theta2_dot^2*sin(theta2) + m3*r1*r2*theta2_dot^2*sin(theta2) + m4*r1*r2*theta2_dot^2*sin(theta2) + m5*r1*r2*theta2_dot^2*sin(theta2) - m3*r2*r3*theta3_dot^2*sin(theta3) + m5*r4*r5*theta5_dot^2*sin(theta5) - L2*m4*r4*theta3_dot^2*sin(theta3 - theta4) - L2*m4*r4*theta4_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta3_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta4_dot^2*sin(theta3 - theta4) + m5*r1*r5*theta2_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m5*r1*r5*theta3_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m5*r1*r5*theta4_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m5*r1*r5*theta5_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m4*r2*r4*theta3_dot^2*sin(theta3 - theta4) + m4*r2*r4*theta4_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta3_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta4_dot^2*sin(theta3 - theta4) - L1*m4*r4*theta2_dot^2*sin(theta2 + theta3 - theta4) - L1*m4*r4*theta3_dot^2*sin(theta2 + theta3 - theta4) - L1*m5*r4*theta2_dot^2*sin(theta2 + theta3 - theta4) - L1*m4*r4*theta4_dot^2*sin(theta2 + theta3 - theta4) - L1*m5*r4*theta3_dot^2*sin(theta2 + theta3 - theta4) - L1*m5*r4*theta4_dot^2*sin(theta2 + theta3 - theta4) - L2*m5*r5*theta3_dot^2*sin(theta3 - theta4 + theta5) - L2*m5*r5*theta4_dot^2*sin(theta3 - theta4 + theta5) - L2*m5*r5*theta5_dot^2*sin(theta3 - theta4 + theta5) - 2*L1*m4*r4*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4) - 2*L1*m4*r4*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4) - 2*L1*m5*r4*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4) + 2*L1*m4*r4*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*L1*m4*r4*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4) - 2*L1*m5*r4*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4) + 2*L1*m4*r4*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*L1*m5*r4*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*L1*m5*r4*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4) + 2*L1*m4*r4*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*L1*m5*r4*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*L1*m5*r4*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*L2*m5*r5*theta1_dot*theta3_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta1_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta2_dot*theta3_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta1_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta2_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta2_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta3_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta3_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta4_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*m4*r1*r4*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4) + 2*m4*r1*r4*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4) + 2*m5*r1*r4*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4) - 2*m4*r1*r4*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*m4*r1*r4*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4) + 2*m5*r1*r4*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4) - 2*m4*r1*r4*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*m5*r1*r4*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*m5*r1*r4*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4) - 2*m4*r1*r4*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*m5*r1*r4*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*m5*r1*r4*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*m5*r2*r5*theta1_dot*theta3_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta1_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta2_dot*theta3_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta1_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta2_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta2_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta3_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta3_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta4_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L1*m3*r3*theta1_dot*theta2_dot*sin(theta2 + theta3) + 2*L1*m3*r3*theta1_dot*theta3_dot*sin(theta2 + theta3) + 2*L1*m3*r3*theta2_dot*theta3_dot*sin(theta2 + theta3) + 2*L1*L2*m2*theta1_dot*theta2_dot*sin(theta2) + 2*L1*L2*m3*theta1_dot*theta2_dot*sin(theta2) + 2*L1*L2*m4*theta1_dot*theta2_dot*sin(theta2) + 2*L1*L2*m5*theta1_dot*theta2_dot*sin(theta2) - 2*m3*r1*r3*theta1_dot*theta2_dot*sin(theta2 + theta3) - 2*m3*r1*r3*theta1_dot*theta3_dot*sin(theta2 + theta3) - 2*m3*r1*r3*theta2_dot*theta3_dot*sin(theta2 + theta3) - 2*L1*m2*r2*theta1_dot*theta2_dot*sin(theta2) - 2*L2*m2*r1*theta1_dot*theta2_dot*sin(theta2) - 2*L1*m3*r2*theta1_dot*theta2_dot*sin(theta2) - 2*L2*m3*r1*theta1_dot*theta2_dot*sin(theta2) - 2*L1*m4*r2*theta1_dot*theta2_dot*sin(theta2) - 2*L2*m4*r1*theta1_dot*theta2_dot*sin(theta2) - 2*L1*m5*r2*theta1_dot*theta2_dot*sin(theta2) - 2*L2*m5*r1*theta1_dot*theta2_dot*sin(theta2) + 2*L2*m3*r3*theta1_dot*theta3_dot*sin(theta3) + 2*L2*m3*r3*theta2_dot*theta3_dot*sin(theta3) - 2*L1*m5*r5*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*L1*m5*r5*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*L1*m5*r5*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*L1*m5*r5*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*L1*m5*r5*theta1_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*L1*m5*r5*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*L1*m5*r5*theta2_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*L1*m5*r5*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*L1*m5*r5*theta3_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*L1*m5*r5*theta4_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m2*r1*r2*theta1_dot*theta2_dot*sin(theta2) + 2*m3*r1*r2*theta1_dot*theta2_dot*sin(theta2) + 2*m4*r1*r2*theta1_dot*theta2_dot*sin(theta2) + 2*m5*r1*r2*theta1_dot*theta2_dot*sin(theta2) - 2*m3*r2*r3*theta1_dot*theta3_dot*sin(theta3) - 2*m3*r2*r3*theta2_dot*theta3_dot*sin(theta3) + 2*m5*r4*r5*theta1_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta2_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta3_dot*theta5_dot*sin(theta5) - 2*m5*r4*r5*theta4_dot*theta5_dot*sin(theta5) - 2*L2*m4*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) - 2*L2*m4*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) - 2*L2*m5*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) - 2*L2*m5*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) + 2*m5*r1*r5*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m5*r1*r5*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*m5*r1*r5*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m5*r1*r5*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m5*r1*r5*theta1_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*m5*r1*r5*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m5*r1*r5*theta2_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*m5*r1*r5*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m5*r1*r5*theta3_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*m5*r1*r5*theta4_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m4*r2*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) + 2*m4*r2*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) + 2*m5*r2*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) + 2*m5*r2*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta3_dot*theta4_dot*sin(theta3 - theta4);
RHS(2,1) = u2 + g*m4*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) + g*m3*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - g*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + g*m2*sin(theta1 + theta2)*(L2 - r2) - m4*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - m5*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + m5*r2*r5*theta3_dot^2*sin(theta3 - theta4 + theta5) + m5*r2*r5*theta4_dot^2*sin(theta3 - theta4 + theta5) + m5*r2*r5*theta5_dot^2*sin(theta3 - theta4 + theta5) - L1*m3*r3*theta1_dot^2*sin(theta2 + theta3) - L1*L2*m2*theta1_dot^2*sin(theta2) - L1*L2*m3*theta1_dot^2*sin(theta2) - L1*L2*m4*theta1_dot^2*sin(theta2) - L1*L2*m5*theta1_dot^2*sin(theta2) + m3*r1*r3*theta1_dot^2*sin(theta2 + theta3) + L1*m2*r2*theta1_dot^2*sin(theta2) + L2*m2*r1*theta1_dot^2*sin(theta2) + L1*m3*r2*theta1_dot^2*sin(theta2) + L2*m3*r1*theta1_dot^2*sin(theta2) + L1*m4*r2*theta1_dot^2*sin(theta2) + L2*m4*r1*theta1_dot^2*sin(theta2) + L1*m5*r2*theta1_dot^2*sin(theta2) + L2*m5*r1*theta1_dot^2*sin(theta2) + L2*m3*r3*theta3_dot^2*sin(theta3) + L1*m5*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) - m2*r1*r2*theta1_dot^2*sin(theta2) - m3*r1*r2*theta1_dot^2*sin(theta2) - m4*r1*r2*theta1_dot^2*sin(theta2) - m5*r1*r2*theta1_dot^2*sin(theta2) - m3*r2*r3*theta3_dot^2*sin(theta3) + m5*r4*r5*theta5_dot^2*sin(theta5) - L2*m4*r4*theta3_dot^2*sin(theta3 - theta4) - L2*m4*r4*theta4_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta3_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta4_dot^2*sin(theta3 - theta4) - m5*r1*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m4*r2*r4*theta3_dot^2*sin(theta3 - theta4) + m4*r2*r4*theta4_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta3_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta4_dot^2*sin(theta3 - theta4) + L1*m4*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + L1*m5*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - L2*m5*r5*theta3_dot^2*sin(theta3 - theta4 + theta5) - L2*m5*r5*theta4_dot^2*sin(theta3 - theta4 + theta5) - L2*m5*r5*theta5_dot^2*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta1_dot*theta3_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta1_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta2_dot*theta3_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta1_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta2_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta2_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta3_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta3_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta4_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta1_dot*theta3_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta1_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta2_dot*theta3_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta1_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta2_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta2_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta3_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta3_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta4_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m3*r3*theta1_dot*theta3_dot*sin(theta3) + 2*L2*m3*r3*theta2_dot*theta3_dot*sin(theta3) - 2*m3*r2*r3*theta1_dot*theta3_dot*sin(theta3) - 2*m3*r2*r3*theta2_dot*theta3_dot*sin(theta3) + 2*m5*r4*r5*theta1_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta2_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta3_dot*theta5_dot*sin(theta5) - 2*m5*r4*r5*theta4_dot*theta5_dot*sin(theta5) - 2*L2*m4*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) - 2*L2*m4*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) - 2*L2*m5*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) - 2*L2*m5*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) + 2*m4*r2*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) + 2*m4*r2*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) + 2*m5*r2*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) + 2*m5*r2*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta3_dot*theta4_dot*sin(theta3 - theta4);
RHS(3,1) = u3 - g*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - g*m4*r4*sin(theta1 + theta2 + theta3 - theta4) - g*m5*r4*sin(theta1 + theta2 + theta3 - theta4) + g*m3*r3*sin(theta1 + theta2 + theta3) - m4*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - m5*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - m5*r2*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) - m5*r2*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) - L1*m3*r3*theta1_dot^2*sin(theta2 + theta3) + m3*r1*r3*theta1_dot^2*sin(theta2 + theta3) - L2*m3*r3*theta1_dot^2*sin(theta3) - L2*m3*r3*theta2_dot^2*sin(theta3) + L1*m5*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m3*r2*r3*theta1_dot^2*sin(theta3) + m3*r2*r3*theta2_dot^2*sin(theta3) + m5*r4*r5*theta5_dot^2*sin(theta5) + L2*m4*r4*theta1_dot^2*sin(theta3 - theta4) + L2*m4*r4*theta2_dot^2*sin(theta3 - theta4) + L2*m5*r4*theta1_dot^2*sin(theta3 - theta4) + L2*m5*r4*theta2_dot^2*sin(theta3 - theta4) - m5*r1*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) - m4*r2*r4*theta1_dot^2*sin(theta3 - theta4) - m4*r2*r4*theta2_dot^2*sin(theta3 - theta4) - m5*r2*r4*theta1_dot^2*sin(theta3 - theta4) - m5*r2*r4*theta2_dot^2*sin(theta3 - theta4) + L1*m4*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + L1*m5*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + L2*m5*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) + L2*m5*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) - 2*L2*m3*r3*theta1_dot*theta2_dot*sin(theta3) + 2*m3*r2*r3*theta1_dot*theta2_dot*sin(theta3) + 2*m5*r4*r5*theta1_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta2_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta3_dot*theta5_dot*sin(theta5) - 2*m5*r4*r5*theta4_dot*theta5_dot*sin(theta5) + 2*L2*m4*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta1_dot*theta2_dot*sin(theta3 - theta4);
RHS(4,:) = u4 + g*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + g*m4*r4*sin(theta1 + theta2 + theta3 - theta4) + g*m5*r4*sin(theta1 + theta2 + theta3 - theta4) + m4*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + m5*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + m5*r2*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) + m5*r2*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) - L1*m5*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) - m5*r4*r5*theta5_dot^2*sin(theta5) - L2*m4*r4*theta1_dot^2*sin(theta3 - theta4) - L2*m4*r4*theta2_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta1_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta2_dot^2*sin(theta3 - theta4) + m5*r1*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m4*r2*r4*theta1_dot^2*sin(theta3 - theta4) + m4*r2*r4*theta2_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta1_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta2_dot^2*sin(theta3 - theta4) - L1*m4*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - L1*m5*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - L2*m5*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) - L2*m5*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) - 2*m5*r4*r5*theta1_dot*theta5_dot*sin(theta5) - 2*m5*r4*r5*theta2_dot*theta5_dot*sin(theta5) - 2*m5*r4*r5*theta3_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta4_dot*theta5_dot*sin(theta5) - 2*L2*m4*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) - 2*L2*m5*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) + 2*m4*r2*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) + 2*m5*r2*r4*theta1_dot*theta2_dot*sin(theta3 - theta4);
RHS(5,:) = u5 - g*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - m5*r2*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) - m5*r2*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) + L1*m5*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) - m5*r4*r5*theta1_dot^2*sin(theta5) - m5*r4*r5*theta2_dot^2*sin(theta5) - m5*r4*r5*theta3_dot^2*sin(theta5) - m5*r4*r5*theta4_dot^2*sin(theta5) - m5*r1*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) + L2*m5*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) + L2*m5*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) - 2*m5*r4*r5*theta1_dot*theta2_dot*sin(theta5) - 2*m5*r4*r5*theta1_dot*theta3_dot*sin(theta5) + 2*m5*r4*r5*theta1_dot*theta4_dot*sin(theta5) - 2*m5*r4*r5*theta2_dot*theta3_dot*sin(theta5) + 2*m5*r4*r5*theta2_dot*theta4_dot*sin(theta5) + 2*m5*r4*r5*theta3_dot*theta4_dot*sin(theta5);

q_acc = inv(M)*RHS;

%[J,Jd] = jacobian(theta_pos,theta_dot);

J=[ 
                                                                                                                                   cos(theta1)*(L1 - r1),                                                                                                                               0,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                                                  -sin(theta1)*(L1 - r1),                                                                                                                               0,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                  cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1),                                                                                                  cos(theta1 + theta2)*(L2 - r2),                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1),                                                                                                 -sin(theta1 + theta2)*(L2 - r2),                                                                                                0,                                                                                                0,                                                   0; ...
                                                               cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) + r3*cos(theta1 + theta2 + theta3),                                                               cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3),                                                                 r3*cos(theta1 + theta2 + theta3),                                                                                                0,                                                   0; ...
                                                             - r3*sin(theta1 + theta2 + theta3) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1),                                                             - r3*sin(theta1 + theta2 + theta3) - sin(theta1 + theta2)*(L2 - r2),                                                                -r3*sin(theta1 + theta2 + theta3),                                                                                                0,                                                   0; ...
                                                      cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4),                                                      cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4),                                                       -r4*cos(theta1 + theta2 + theta3 - theta4),                                                        r4*cos(theta1 + theta2 + theta3 - theta4),                                                   0; ...
                                                      r4*sin(theta1 + theta2 + theta3 - theta4) - sin(theta1)*(L1 - r1) - sin(theta1 + theta2)*(L2 - r2),                                                      r4*sin(theta1 + theta2 + theta3 - theta4) - sin(theta1 + theta2)*(L2 - r2),                                                        r4*sin(theta1 + theta2 + theta3 - theta4),                                                       -r4*sin(theta1 + theta2 + theta3 - theta4),                                                   0; ...
 cos(theta1 + theta2)*(L2 - r2) - r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4), cos(theta1 + theta2)*(L2 - r2) - r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - r4*cos(theta1 + theta2 + theta3 - theta4), - r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - r4*cos(theta1 + theta2 + theta3 - theta4),   r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4), -r5*cos(theta1 + theta2 + theta3 - theta4 + theta5); ...
 r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1) + r4*sin(theta1 + theta2 + theta3 - theta4), r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4),   r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4), - r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - r4*sin(theta1 + theta2 + theta3 - theta4),  r5*sin(theta1 + theta2 + theta3 - theta4 + theta5); ...
                                                                                                                                                      -1,                                                                                                                               0,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                               -1,                                                                                                0,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                               -1,                                                                                                1,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                               -1,                                                                                                1,                                                  -1];
 


Jd =[                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               -theta1_dot*sin(theta1)*(L1 - r1),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0; ...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               -theta1_dot*cos(theta1)*(L1 - r1),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               - theta1_dot*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1)) - theta2_dot*sin(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 - theta1_dot*sin(theta1 + theta2)*(L2 - r2) - theta2_dot*sin(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1)) - theta2_dot*cos(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 - theta1_dot*cos(theta1 + theta2)*(L2 - r2) - theta2_dot*cos(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1)) - theta2_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - r3*theta3_dot*sin(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - theta2_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - r3*theta3_dot*sin(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                             - r3*theta1_dot*sin(theta1 + theta2 + theta3) - r3*theta2_dot*sin(theta1 + theta2 + theta3) - r3*theta3_dot*sin(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) + r3*cos(theta1 + theta2 + theta3)) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3)) - r3*theta3_dot*cos(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3)) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3)) - r3*theta3_dot*cos(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                             - r3*theta1_dot*cos(theta1 + theta2 + theta3) - r3*theta2_dot*cos(theta1 + theta2 + theta3) - r3*theta3_dot*cos(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                         r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - theta2_dot*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1) - r4*sin(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                         r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - theta2_dot*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta1_dot*sin(theta1 + theta2 + theta3 - theta4) + r4*theta2_dot*sin(theta1 + theta2 + theta3 - theta4) + r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta2_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta1_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                         r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                         r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta1_dot*cos(theta1 + theta2 + theta3 - theta4) + r4*theta2_dot*cos(theta1 + theta2 + theta3 - theta4) + r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta2_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta1_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                                                             0;...
 theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), r5*theta1_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta2_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta3_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) - r5*theta4_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5);...
 theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) - cos(theta1)*(L1 - r1) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) + r4*cos(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) + r4*cos(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), r5*theta1_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta2_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta3_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) - r5*theta4_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5);...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0];
 


vel = J*qd;

angvel= [vel(11,1);vel(12,1);vel(13,1);vel(14,1);vel(15,1)]; % acceleration in x direction

acc = J*q_acc +Jd*qd;

x_acc = [acc(1); acc(3); acc(5); acc(7); acc(9)]; % acceleration in x direction
y_acc = [acc(2); acc(4); acc(6); acc(8); acc(10)]; % acceleration in y direction

[pm, pcm] = computeFK(theta);


num = 0;
den = 0;
for i=1:5
    num = num + m(i)*( pcm(1,i)*(y_acc(i)+g) - pcm(2,i)*x_acc(i)) - Izz(i)*angvel(i) ; % zmp numerator
    den = den + m(i) * (y_acc(i) + g); % zmp denominator
end
zmp=num/den;
end

